//
// Created by wujehy on 18-11-14.
//

#include "Steering.h"
#include "Servo.h"

Servo myservoh;
Servo myservov;



void Steering::Init() {
    myservov.attach(9);
    myservoh.attach(10);
    setMon_Y(90);
    setMon_X(90);
    DoY();
    DoX();
}

void Steering::Up() {
    setMon_X(Mon_X-level);
    DoX();
}

void Steering::Down() {
    setMon_X(Mon_X+level);
  DoX();
}


void Steering::Left() {
    setMon_Y(Mon_Y+level);
        DoY();
}

void Steering::Right() {
    setMon_Y(Mon_Y-level);
   DoY();

}


void Steering::DoX() {
    myservoh.write(Mon_X);
}

void Steering::DoY() {
    myservov.write(Mon_Y);
}

Steering *Steering::MoreLevel() {
    setLevel(30);
    return getObj();
}

Steering * Steering::MiddleLevel() {
    setLevel(15);
    return getObj();
}



Steering *Steering::HighLevel() {
    setLevel(10);
    return getObj();
}

Steering *Steering::LowLevel() {
    setLevel(5);
    return getObj();
}
void Steering::setMon_X(unsigned char Mon_X) {
    if (Mon_X>=0&&Mon_X<=180){
        Steering::Mon_X = Mon_X;
    } else if(Mon_X<0||Mon_X>=225){
        Steering::Mon_X=0;
    } else if(Mon_X>180||Mon_X<=224){
        Steering::Mon_X=180;
    }


}

void Steering::setMon_Y(unsigned char Mon_Y) {
    if (Mon_Y>=0&&Mon_Y<=180){
        Steering::Mon_Y = Mon_Y;
    } else if(Mon_Y<0||Mon_Y>225){
        Steering::Mon_Y=0;
    } else if(Mon_Y>180||Mon_Y<224){
        Steering::Mon_Y=180;
    }

}

void Steering::setLevel(unsigned char level) {
    Steering::level = level;
}

Steering *Steering::getObj() const {
    return (Steering *)this;
}
